#include "balance.h"

int Time_count=0; 



int robot_mode_check_flag=0; 
u8 command_lost_count=0;
short test_num;

Encoder OriginalEncoder; 

void Drive_Motor(float Vx,float Vy,float Vz)
{
		float amplitude=3.5; 
	
	  
	  
	  if(Car_Mode==Mec_Car)
		{
			Smooth_control(Vx,Vy,Vz); 
  
      
			
			Vx=smooth_control.VX;     
			Vy=smooth_control.VY;
			Vz=smooth_control.VZ;
		}
		
		
	  
	  if (Car_Mode==Mec_Car) 
    {
			
			MOTOR_A.Target   = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
			MOTOR_B.Target   = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
			MOTOR_C.Target   = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
			MOTOR_D.Target   = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
		
			
			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
			MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); 
			MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude); 
		} 
		
		
		
		else if(Car_Mode==FourWheel_Car) 
		{	
			
			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; 
			MOTOR_B.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; 
			MOTOR_C.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; 
			MOTOR_D.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; 
					
			
			MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
			MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
			MOTOR_C.Target=target_limit_float( MOTOR_C.Target,-amplitude,amplitude); 
			MOTOR_D.Target=target_limit_float( MOTOR_D.Target,-amplitude,amplitude); 	
		}
		
		
		
		else if (Car_Mode==Tank_Car) 
		{
			
			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing) / 2.0f;    
		  MOTOR_B.Target =  Vx + Vz * (Wheel_spacing) / 2.0f;    
			
			
		  MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
	    MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
			MOTOR_C.Target=0; 
			MOTOR_D.Target=0; 
		}

		
}

void Balance_task(void *pvParameters)
{ 

    while(1)
    {	
			
			
			LOS_Msleep(10); 
			
			
			
			if(Time_count<3000)Time_count++;
			
			
			
			
			Get_Velocity_Form_Encoder();   
			
			if(Servo_init_angle_adjust == 1) Servo_init_angle_adjust_function(); 
			
			if(Check==0) 
			{
				if(APP_ON_Flag)
				{
					Get_RC();  
				}else if (PS2_ON_Flag){
					//printf("____>>>>> %s %d \r\n", __FILE__, __LINE__);
					PS2_control(); 
				}
				else{
					Drive_Motor(Move_X, Move_Y, Move_Z);
				}                      
				
				
				
				Key(); 
				
				Drive_Robot_Arm();
				
				
        
				
				if(Turn_Off(Voltage)==0) 
				 { 			
           
					 
					 
					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
					 
					 Limit_Pwm(16700);
					 
						 
					 
					 
					 
					 switch(Car_Mode)
					 {
							case Mec_Car:       Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,   MOTOR_C.Motor_Pwm, -MOTOR_D.Motor_Pwm,Position1,Position2,Position3,Position4); break; 
							case FourWheel_Car: Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,   MOTOR_C.Motor_Pwm, -MOTOR_D.Motor_Pwm,Position1,Position2,Position3,Position4); break; 
							case Tank_Car:      Set_Pwm(-MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm,  -MOTOR_C.Motor_Pwm, -MOTOR_D.Motor_Pwm,Position1,Position2,Position3,Position4); break; 
					 }
				 }
				 
				 
				 else	Set_Pwm(0,0,0,0,0,0,0,0); 
			 }	
		 }  
}

void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo1,int servo2,int servo3,int servo4)
{
	
	//printf("____>>>>> %s %d motor_a %d\r\n", __FILE__, __LINE__, motor_a);
	//printf("____>>>>> %s %d motor_b %d\r\n", __FILE__, __LINE__, motor_b);
	//printf("____>>>>> %s %d motor_c %d\r\n", __FILE__, __LINE__, motor_c);
	//printf("____>>>>> %s %d motor_d %d\r\n", __FILE__, __LINE__, motor_d);

	//printf("____>>>>> %s %d servo1 %d\r\n", __FILE__, __LINE__, servo1);
	//printf("____>>>>> %s %d servo2 %d\r\n", __FILE__, __LINE__, servo2);
	//printf("____>>>>> %s %d servo3 %d\r\n", __FILE__, __LINE__, servo3);
	//printf("____>>>>> %s %d servo4 %d\r\n", __FILE__, __LINE__, servo4);

	if(motor_a<0)			PWMA1=16799,PWMA2=16799+motor_a;
	else 	            PWMA2=16799,PWMA1=16799-motor_a;
	
	
	
	if(motor_b<0)			PWMB1=16799,PWMB2=16799+motor_b;
	else 	            PWMB2=16799,PWMB1=16799-motor_b;


	
	
	if(motor_c<0)			PWMC1=16799,PWMC2=16799+motor_c;
	else 	            PWMC2=16799,PWMC1=16799-motor_c;
	
	
	
	if(motor_d<0)			PWMD1=16799,PWMD2=16799+motor_d;
	else 	            PWMD2=16799,PWMD1=16799-motor_d;
	
	
	
	Servo_PWM1 =servo1;
	Servo_PWM2 =servo2;
	Servo_PWM3 =servo3;
	Servo_PWM4 =servo4;

}


void Limit_Pwm(int amplitude)
{	
	    MOTOR_A.Motor_Pwm=target_limit_float(MOTOR_A.Motor_Pwm,-amplitude,amplitude);
	    MOTOR_B.Motor_Pwm=target_limit_float(MOTOR_B.Motor_Pwm,-amplitude,amplitude);
		  MOTOR_C.Motor_Pwm=target_limit_float(MOTOR_C.Motor_Pwm,-amplitude,amplitude);
	    MOTOR_D.Motor_Pwm=target_limit_float(MOTOR_D.Motor_Pwm,-amplitude,amplitude);
}	    

float target_limit_float(float insert,float low,float high)
{
    if (insert < low)
        return low;
    else if (insert > high)
        return high;
    else
        return insert;	
}
int target_limit_int(int insert,int low,int high)
{
    if (insert < low)
        return low;
    else if (insert > high)
        return high;
    else
        return insert;	
}

u8 Turn_Off( int voltage)
{
	    u8 temp;
			if(voltage<10||EN==0||Flag_Stop==1)
			{	                                                
				temp=1;      
				PWMA1=0;PWMA2=0;
				PWMB1=0;PWMB2=0;		
				PWMC1=0;PWMC1=0;	
				PWMD1=0;PWMD2=0;					
      }
			else
			temp=0;
			return temp;			
}

u32 myabs(long int a)
{ 		   
	  u32 temp;
		if(a<0)  temp=-a;  
	  else temp=a;
	  return temp;
}

int Incremental_PI_A (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; 
	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
	 if(Pwm>16700)Pwm=16700;
	 if(Pwm<-16700)Pwm=-16700;
	 Last_bias=Bias; 
	 return Pwm;    
}
int Incremental_PI_B (float Encoder,float Target)
{  
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; 
	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;  
	 if(Pwm>16700)Pwm=16700;
	 if(Pwm<-16700)Pwm=-16700;
	 Last_bias=Bias; 
	 return Pwm;
}
int Incremental_PI_C (float Encoder,float Target)
{  
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; 
	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
	 if(Pwm>16700)Pwm=16700;
	 if(Pwm<-16700)Pwm=-16700;
	 Last_bias=Bias; 
	 return Pwm; 
}
int Incremental_PI_D (float Encoder,float Target)
{  
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; 
	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;  
	 if(Pwm>16700)Pwm=16700;
	 if(Pwm<-16700)Pwm=-16700;
	 Last_bias=Bias; 
	 return Pwm; 
}


float Position_PID1 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                           
}
float Position_PID2 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                           
}
float Position_PID3 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                           
}
float Position_PID4 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                           
}
float Position_PID5 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                           
}
float Position_PID6 (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-Encoder;                                  
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                           
}

void Get_RC(void)
{
	u8 Flag_Move=1;
	float step=0.003;
	if(car_A_steer_flag==1)
	{
	if(Car_Mode==Mec_Car) 
	{
	 switch(Flag_Direction)  
	 { 
			case 1:      Move_X=RC_Velocity;  	 Move_Y=0;             Flag_Move=1;    break;
			case 2:      Move_X=RC_Velocity;  	 Move_Y=-RC_Velocity;  Flag_Move=1; 	 break;
			case 3:      Move_X=0;      		     Move_Y=-RC_Velocity;  Flag_Move=1; 	 break;
			case 4:      Move_X=-RC_Velocity;  	 Move_Y=-RC_Velocity;  Flag_Move=1;    break;
			case 5:      Move_X=-RC_Velocity;  	 Move_Y=0;             Flag_Move=1;    break;
			case 6:      Move_X=-RC_Velocity;  	 Move_Y=RC_Velocity;   Flag_Move=1;    break;
			case 7:      Move_X=0;     	 		     Move_Y=RC_Velocity;   Flag_Move=1;    break;
			case 8:      Move_X=RC_Velocity; 	   Move_Y=RC_Velocity;   Flag_Move=1;    break; 
			default:     Move_X=0;               Move_Y=0;             Flag_Move=0;    break;
	 }
	 if(Flag_Move==0)		
	 {	
		 
		 
		 if     (Flag_Left ==1)  Move_Z= PI/2*(RC_Velocity/500); 
		 else if(Flag_Right==1)  Move_Z=-PI/2*(RC_Velocity/500); 
		 else 		               Move_Z=0;                       
	 }
	}	
	else 
	{
	 switch(Flag_Direction) 
	 { 
			case 1:      Move_X=+RC_Velocity;  	 Move_Z=0;         break;
			case 2:      Move_X=+RC_Velocity;  	 Move_Z=-PI/2;   	 break;
			case 3:      Move_X=0;      				 Move_Z=-PI/2;   	 break;	 
			case 4:      Move_X=-RC_Velocity;  	 Move_Z=-PI/2;     break;		 
			case 5:      Move_X=-RC_Velocity;  	 Move_Z=0;         break;	 
			case 6:      Move_X=-RC_Velocity;  	 Move_Z=+PI/2;     break;	 
			case 7:      Move_X=0;     	 			 	 Move_Z=+PI/2;     break;
			case 8:      Move_X=+RC_Velocity; 	 Move_Z=+PI/2;     break; 
			default:     Move_X=0;               Move_Z=0;         break;
	 }
	 if     (Flag_Left ==1)  Move_Z= PI/2; 
	 else if(Flag_Right==1)  Move_Z=-PI/2; 
	}
	
 if(Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
	{
	  if(Move_X<0) Move_Z=-Move_Z; 
		Move_Z=Move_Z*RC_Velocity/500;
	}
}	
	else if (car_A_steer_flag==2)
	{
	 switch(Flag_Direction) 
	 { 
			case 1:      Moveit_Angle1=Moveit_Angle1+step;    Flag_Move=1;    break;
			case 2:      Moveit_Angle2=Moveit_Angle2+step;    Flag_Move=1;    break;
			case 3:      Moveit_Angle3=Moveit_Angle3+step;    Flag_Move=1;    break;
			case 4:      Moveit_Angle4=Moveit_Angle4+step;    Flag_Move=1;    break;
			case 5:      Moveit_Angle1=Moveit_Angle1-step;    Flag_Move=1;    break;
			case 6:      Moveit_Angle2=Moveit_Angle2-step;    Flag_Move=1;    break;
			case 7:      Moveit_Angle3=Moveit_Angle3-step;    Flag_Move=1;    break;
			case 8:      Moveit_Angle4=Moveit_Angle4-step;    Flag_Move=1;    break;
	 }
		 Moveit_Angle1=target_limit_float(Moveit_Angle1,-1.57,1.57);
		 Moveit_Angle2=target_limit_float(Moveit_Angle2,-1.57,1.57);
		 Moveit_Angle3=target_limit_float(Moveit_Angle3,-1.57,1.57);
		 Moveit_Angle4=target_limit_float(Moveit_Angle4,-1.57,1.57);
		
	}
	
  
	Move_X=Move_X/1000;       Move_Y=Move_Y/1000;         Move_Z=Move_Z;
	
	
	
	Drive_Motor(Move_X,Move_Y,Move_Z);
}


void PS2_control(void)
{
   	int LX,LY,RY;
		int Threshold=20; 
		float step1=0.005;	
	  

		LY=-(PS2_LX-128);  
		LX=-(PS2_LY-128); 
		RY=-(PS2_RX-128); 
	
		//printf("____>>>>> %s %d LY %d\r\n", __FILE__, __LINE__, LY);
	  	//printf("____>>>>> %s %d LX %d\r\n", __FILE__, __LINE__, LX);
		//printf("____>>>>> %s %d RY %d\r\n", __FILE__, __LINE__, RY);

		if(LX>-Threshold&&LX<Threshold)LX=0; 
		if(LY>-Threshold&&LY<Threshold)LY=0; 
		if(RY>-Threshold&&RY<Threshold)RY=0; 
	
	  	//printf("____>>>>> %s %d LY %d\r\n", __FILE__, __LINE__, LY);
	  	//printf("____>>>>> %s %d LX %d\r\n", __FILE__, __LINE__, LX);
		//printf("____>>>>> %s %d RY %d\r\n", __FILE__, __LINE__, RY);
	  
		Move_X=LX*RC_Velocity/128; 
		Move_Y=LY*RC_Velocity/128; 
	  	Move_Z=RY*(PI/2)/128;      
	
		//printf("____>>>>> %s %d Move_X %d\r\n", __FILE__, __LINE__, Move_X);
	  	//printf("____>>>>> %s %d Move_Y %d\r\n", __FILE__, __LINE__, Move_Y);
		//printf("____>>>>> %s %d Move_Z %d\r\n", __FILE__, __LINE__, Move_Z);
	  
	  if(Car_Mode==Mec_Car)
		{
			Move_Z=Move_Z*RC_Velocity/500;
		}	

		else if(Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
		{
			if(Move_X<0) Move_Z=-Move_Z; 
			Move_Z=Move_Z*RC_Velocity/500;
		}	
		 
	  
    
		Move_X=Move_X/1000;        
		Move_Y=Move_Y/1000;    
		Move_Z=Move_Z;
		
		if(PS2_LX<100 && PS2_RX>200)          Move_X=0,Move_Y=0,Move_Z=0;  
		else 		if(PS2_LX>200 && PS2_RX<100)  Move_X=0,Move_Y=0,Move_Z=0;  
		
		//printf("____>>>>> %s %d Move_X %d\r\n", __FILE__, __LINE__, Move_X);
	  	//printf("____>>>>> %s %d Move_Y %d\r\n", __FILE__, __LINE__, Move_Y);
		//printf("____>>>>> %s %d Move_Z %d\r\n", __FILE__, __LINE__, Move_Z);

		if(Servo_init_angle_adjust == 0)
		{
			//printf("____>>>>> %s %d PS2_KEY %d\r\n", __FILE__, __LINE__, PS2_KEY);
			switch(PS2_KEY)   
			 { 
			case 5:       Moveit_Angle1=Moveit_Angle1-step1;  break;       
			case 6:       Moveit_Angle2=Moveit_Angle2+step1;  break;       
			case 13:      Moveit_Angle3=Moveit_Angle3+step1;  break;     
			case 14:      Moveit_Angle4=Moveit_Angle4+step1;  break;     
			case 7:       Moveit_Angle1=Moveit_Angle1+step1;  break;   
			case 8:       Moveit_Angle2=Moveit_Angle2-step1;  break;    
			case 15:      Moveit_Angle3=Moveit_Angle3-step1;  break;   
			case 16:      Moveit_Angle4=Moveit_Angle4-step1;  break;   
			case 11:      RC_Velocity+=5;                     break; 
			case 9:       RC_Velocity-=5;                     break; 
			default:        break;
			 }

	  }
		 Moveit_Angle1=target_limit_float(Moveit_Angle1,-1.57,1.57);
		 Moveit_Angle2=target_limit_float(Moveit_Angle2,-1.57,1.57);
		 Moveit_Angle3=target_limit_float(Moveit_Angle3,-1.57,1.57);
		 Moveit_Angle4=target_limit_float(Moveit_Angle4,-1.57,1.57);

		Drive_Motor(Move_X,Move_Y,Move_Z);		 			
} 


void Remote_Control(void)
{
	  
	  
    static u8 thrice=100; 
    int Threshold=100; 
	  
    int LX,LY,RY,RX,Remote_RCvelocity; 
		Remoter_Ch1=target_limit_int(Remoter_Ch1,1000,2000);
		Remoter_Ch2=target_limit_int(Remoter_Ch2,1000,2000);
		Remoter_Ch3=target_limit_int(Remoter_Ch3,1000,2000);
		Remoter_Ch4=target_limit_int(Remoter_Ch4,1000,2000);

	  
	  
    LX=Remoter_Ch2-1500; 
	
	  
	  
	  
    LY=Remoter_Ch4-1500;

    
		
	  RX=Remoter_Ch3-1500;

    
		
    RY=Remoter_Ch1-1500; 

    if(LX>-Threshold&&LX<Threshold)LX=0;
    if(LY>-Threshold&&LY<Threshold)LY=0;
    if(RX>-Threshold&&RX<Threshold)RX=0;
	  if(RY>-Threshold&&RY<Threshold)RY=0;
		
		
		Remote_RCvelocity=RC_Velocity+RX;
	  if(Remote_RCvelocity<0)Remote_RCvelocity=0;
		
		
		
    Move_X= LX*Remote_RCvelocity/500; 
		Move_Y=-LY*Remote_RCvelocity/500;
		Move_Z=-RY*(PI/2)/500;      
			 
		
	  if(Car_Mode==Mec_Car)
		{
			Move_Z=Move_Z*Remote_RCvelocity/500;
		}	
		else if(Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
		{
			if(Move_X<0) Move_Z=-Move_Z; 
			Move_Z=Move_Z*Remote_RCvelocity/500;
		}
	  
    
		Move_X=Move_X/1000;       
    Move_Y=Move_Y/1000;      
		Move_Z=Move_Z;
		
	  
	  
    if(thrice>0) Move_X=0,Move_Z=0,thrice--;
				
		
	  
		Drive_Motor(Move_X,Move_Y,Move_Z);
}

void Key(void)
{	
	u8 tmp;
	tmp=click_N_Double_MPU6050(50); 
	if(tmp==2)memcpy(Deviation_gyro,Original_gyro,sizeof(gyro)),memcpy(Deviation_accel,Original_accel,sizeof(accel));
}

void Get_Velocity_Form_Encoder(void)
{
	  
	  
		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; 
		OriginalEncoder.A=-Read_Encoder(2);	
		OriginalEncoder.B=-Read_Encoder(3);	
		OriginalEncoder.C=-Read_Encoder(4);	
		OriginalEncoder.D=-Read_Encoder(5);	

	
	
	  
		
		switch(Car_Mode)
		{
			case Mec_Car:       Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C;  Encoder_D_pr=-OriginalEncoder.D; break; 
			case FourWheel_Car: Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C;  Encoder_D_pr=-OriginalEncoder.D; break; 
			case Tank_Car:      Encoder_A_pr=OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break; 
		}
		
		
		
		MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
		MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
		MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
		MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
}

void Smooth_control(float vx,float vy,float vz)
{
	float step=0.01;

	if	   (vx>0) 	smooth_control.VX+=step;
	else if(vx<0)		smooth_control.VX-=step;
	else if(vx==0)	smooth_control.VX=smooth_control.VX*0.9f;
	
	if	   (vy>0)   smooth_control.VY+=step;
	else if(vy<0)		smooth_control.VY-=step;
	else if(vy==0)	smooth_control.VY=smooth_control.VY*0.9f;
	
	if	   (vz>0) 	smooth_control.VZ+=step;
	else if(vz<0)		smooth_control.VZ-=step;
	else if(vz==0)	smooth_control.VZ=smooth_control.VZ*0.9f;
	
	smooth_control.VX=target_limit_float(smooth_control.VX,-float_abs(vx),float_abs(vx));
	smooth_control.VY=target_limit_float(smooth_control.VY,-float_abs(vy),float_abs(vy));
	smooth_control.VZ=target_limit_float(smooth_control.VZ,-float_abs(vz),float_abs(vz));
}

float float_abs(float insert)
{
	if(insert>=0) return insert;
	else return -insert;
}

void robot_mode_check(void)
{
	static u8 error=0;

	if(abs(MOTOR_A.Motor_Pwm)>2500||abs(MOTOR_B.Motor_Pwm)>2500||abs(MOTOR_C.Motor_Pwm)>2500||abs(MOTOR_D.Motor_Pwm)>2500)   error++;
	
	
	if(error>6) EN=0,Flag_Stop=1,robot_mode_check_flag=1;  
}


int SERVO_PWM_VALUE(float angle)
{
			int K=1000;
			float Ratio=0.6369;
			int pwm_value;
			
			pwm_value=(SERVO_INIT-angle*K*Ratio); 
	    return pwm_value;
}

void moveit_angle_limit(void)
{ 
	Moveit_Angle1=target_limit_float(Moveit_Angle1,-1.57,1.57);
	Moveit_Angle2=target_limit_float(Moveit_Angle2,-1.57,1.57);
	Moveit_Angle3=target_limit_float(Moveit_Angle3,-1.57,1.57);
	Moveit_Angle4=target_limit_float(Moveit_Angle4,-0.7,0.7);
}

void moveit_pwm_limit(void)
{ 
	Moveit_PWM1=target_limit_int(Moveit_PWM1,400,2600);
	Moveit_PWM2=target_limit_int(Moveit_PWM2,400,2600);
	Moveit_PWM3=target_limit_int(Moveit_PWM3,400,2600);
	Moveit_PWM4=target_limit_int(Moveit_PWM4,900,2100);  
}


void Drive_Robot_Arm(void)
{
		  moveit_angle_limit();		 
			Moveit_PWM1=  SERVO_PWM_VALUE(Moveit_Angle1)+Moveit_Angle1_init; 
			Moveit_PWM2 = SERVO_PWM_VALUE(Moveit_Angle2)+Moveit_Angle2_init;
			Moveit_PWM3 = SERVO_PWM_VALUE(Moveit_Angle3)+Moveit_Angle3_init;
			Moveit_PWM4 = SERVO_PWM_VALUE(Moveit_Angle4)+Moveit_Angle4_init;
	    moveit_pwm_limit();
	
			Velocity1=Position_PID1(Position1,Moveit_PWM1);
	    Velocity2=Position_PID2(Position2,Moveit_PWM2);
	    Velocity3=Position_PID3(Position3,Moveit_PWM3);
	    Velocity4=Position_PID4(Position4,Moveit_PWM4);
	    

      Position1+=Velocity1;		   
      Position2+=Velocity2;
      Position3+=Velocity3;
	    Position4+=Velocity4;
	
}
